﻿using SoftFloat;
using System;
using System.Diagnostics;
using System.Diagnostics.CodeAnalysis;
using System.Globalization;
using System.Runtime.CompilerServices;
using System.Runtime.InteropServices;
using System.Text;

namespace BepuUtilities
{
    public unsafe static class Vector128
    {
        public static bool IsHardwareAccelerated { get => true; }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector128<T> BitwiseAnd<T>(Vector128<T> left, Vector128<T> right)
        {
            return left & right;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector128<T> BitwiseOr<T>(Vector128<T> left, Vector128<T> right)
        {
            return left | right;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector128<T> Create<T>(Vector64<T> lower, Vector64<T> upper)
        {
            Unsafe.SkipInit(out Vector128<T> result);
            result.SetLowerUnsafe(lower);
            result.SetUpperUnsafe(upper);
            return result;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        internal unsafe static void SetLowerUnsafe<T>(ref this Vector128<T> vector, Vector64<T> value)
        {
            Unsafe.AsRef(ref vector._lower) = value;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        internal unsafe static void SetUpperUnsafe<T>(ref this Vector128<T> vector, Vector64<T> value)
        {
            Unsafe.AsRef(ref vector._upper) = value;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        internal unsafe static ref T GetElementUnsafe<T>(ref this Vector128<T> vector, int index)
        {
            ref T source = ref Unsafe.As<Vector128<T>, T>(ref Unsafe.AsRef(ref vector));
            return ref Unsafe.Add(ref source, index);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        internal unsafe static void SetElementUnsafe<T>(ref this Vector128<T> vector, int index, T value)
        {
            ref T source = ref Unsafe.As<Vector128<T>, T>(ref Unsafe.AsRef(ref vector));
            Unsafe.Add(ref source, index) = value;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector128<sfloat> Create(sfloat value)
        {
            return Create<sfloat>(value);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector128<T> Create<T>(T value)
        {
            Vector64<T> vector = Vector64.Create(value);
            return Create(vector, vector);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector128<int> Create(int value)
        {
            return Create<int>(value);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector128<int> Create(int e0, int e1, int e2, int e3)
        {
            return Create(Vector64.Create(e0, e1), Vector64.Create(e2, e3));
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector128<uint> Create(uint e0, uint e1, uint e2, uint e3)
        {
            return Create(Vector64.Create(e0, e1), Vector64.Create(e2, e3));
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static uint ExtractMostSignificantBits<T>(this Vector128<T> vector)
        {
            uint num = vector._lower.ExtractMostSignificantBits();
            return num | vector._upper.ExtractMostSignificantBits() << Vector64<T>.Count;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector128<T> LessThan<T>(Vector128<T> left, Vector128<T> right)
        {
            return Create(Vector64.LessThan(left._lower, right._lower), Vector64.LessThan(left._upper, right._upper));
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector128<T> LessThanOrEqual<T>(Vector128<T> left, Vector128<T> right)
        {
            return Create(Vector64.LessThanOrEqual(left._lower, right._lower), Vector64.LessThanOrEqual(left._upper, right._upper));
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public unsafe static Vector128<T> Load<T>(T* source)
        {
            return LoadUnsafe(ref *source);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector128<T> LoadUnsafe<T>(ref T source)
        {
            ref byte source2 = ref Unsafe.As<T, byte>(ref Unsafe.AsRef(ref source));
            return Unsafe.ReadUnaligned<Vector128<T>>(ref source2);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector128<T> Max<T>(Vector128<T> left, Vector128<T> right)
        {
            return Create(Vector64.Max(left._lower, right._lower), Vector64.Max(left._upper, right._upper));
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector128<T> Min<T>(Vector128<T> left, Vector128<T> right)
        {
            return Create(Vector64.Min(left._lower, right._lower), Vector64.Min(left._upper, right._upper));
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public unsafe static void Store<T>(this Vector128<T> source, T* destination)
        {
            source.StoreUnsafe(ref *destination);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void StoreUnsafe<T>(this Vector128<T> source, ref T destination)
        {
            ref byte destination2 = ref Unsafe.As<T, byte>(ref destination);
            Unsafe.WriteUnaligned(ref destination2, source);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector128<int> AsInt32<T>(this Vector128<T> vector)
        {
            return vector.As<T, int>();
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public unsafe static Vector128<TTo> As<TFrom, TTo>(this Vector128<TFrom> vector)
        {
            return Unsafe.As<Vector128<TFrom>, Vector128<TTo>>(ref vector);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector128<sfloat> AsSingle<T>(this Vector128<T> vector)
        {
            return vector.As<T, sfloat>();
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector128<sfloat> AsVector128(this Vector3 value)
        {
            return new Vector4(value, 0f).AsVector128();
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public unsafe static Vector128<sfloat> AsVector128(this Vector4 value)
        {
            return Unsafe.As<Vector4, Vector128<sfloat>>(ref value);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector128<T> Equals<T>(Vector128<T> left, Vector128<T> right)
        {
            return Create(Vector64.Equals(left._lower, right._lower), Vector64.Equals(left._upper, right._upper));
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector128<T> ConditionalSelect<T>(Vector128<T> condition, Vector128<T> left, Vector128<T> right)
        {
            return Create(Vector64.ConditionalSelect(condition._lower, left._lower, right._lower), Vector64.ConditionalSelect(condition._upper, left._upper, right._upper));
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public unsafe static Vector4 AsVector4(this Vector128<sfloat> value)
        {
            return Unsafe.As<Vector128<sfloat>, Vector4>(ref value);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<T> AsVector<T>(this Vector128<T> value)
        {
            Vector<T> result = default;
            Unsafe.WriteUnaligned(ref Unsafe.As<Vector<T>, byte>(ref result), value);
            return result;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector256<T> ToVector256Unsafe<T>(this Vector128<T> vector)
        {
            Unsafe.SkipInit(out Vector256<T> result);
            result.SetLowerUnsafe(vector);
            return result;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector128<T> AndNot<T>(Vector128<T> left, Vector128<T> right)
        {
            return Create(Vector64.AndNot(left._lower, right._lower), Vector64.AndNot(left._upper, right._upper));
        }
    }

    [DebuggerTypeProxy(typeof(Vector128DebugView<>))]
    [StructLayout(LayoutKind.Sequential, Size = 16)]
    public struct Vector128<T>
    {
        internal Vector64<T> _lower;
        internal Vector64<T> _upper;

        public static Vector128<T> Zero
        {
            get
            {
                return default;
            }
        }
        public static Vector128<T> AllBitsSet
        {
            get
            {
                Vector64<T> allBitsSet = Vector64<T>.AllBitsSet;
                return Vector128.Create(allBitsSet, allBitsSet);
            }
        }
        public static int Count
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get
            {
                return Vector64<T>.Count * 2;
            }
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector128<T> operator +(Vector128<T> left, Vector128<T> right)
        {
            return Vector128.Create(left._lower + right._lower, left._upper + right._upper);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector128<T> operator |(Vector128<T> left, Vector128<T> right)
        {
            return Vector128.Create(left._lower | right._lower, left._upper | right._upper);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector128<T> operator -(Vector128<T> left, Vector128<T> right)
        {
            return Vector128.Create(left._lower - right._lower, left._upper - right._upper);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector128<T> operator &(Vector128<T> left, Vector128<T> right)
        {
            return Vector128.Create(left._lower & right._lower, left._upper & right._upper);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector128<T> operator ^(Vector128<T> left, Vector128<T> right)
        {
            return Vector128.Create(left._lower ^ right._lower, left._upper ^ right._upper);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public override string ToString()
        {
            return ToString("G", CultureInfo.InvariantCulture);
        }

        private unsafe string ToString([StringSyntax("NumericFormat")] string format, IFormatProvider formatProvider)
        {
            var valueStringBuilder = new StringBuilder();
            string numberGroupSeparator = NumberFormatInfo.GetInstance(formatProvider).NumberGroupSeparator;
            valueStringBuilder.Append('<');
            valueStringBuilder.Append(((IFormattable)(object)this.GetElementUnsafe(0)).ToString(format, formatProvider));
            for (int i = 1; i < Count; i++)
            {
                valueStringBuilder.Append(numberGroupSeparator);
                valueStringBuilder.Append(' ');
                valueStringBuilder.Append(((IFormattable)(object)this.GetElementUnsafe(i)).ToString(format, formatProvider));
            }
            valueStringBuilder.Append('>');
            return valueStringBuilder.ToString();
        }
    }
}